/*
 * CollisionWorld detects and handles the line segment intersections
 */

#include <iostream>
#include <stdlib.h>
#include <math.h>
#include <assert.h>
#include <stdio.h>
#include <list>

#include "CollisionWorld.h"
#include "IntersectionDetection.h"
#include "Line.h"
#include "QuadTree.h"
#include <cilk/cilk.h>

// Constructor.
CollisionWorld::CollisionWorld()
{
   numLineWallCollisions = 0;
   numLineLineCollisions = 0;
   timeStep = 0.5;
   qt = new QuadTree(Vec(BOX_XMIN, BOX_YMIN), Vec(BOX_XMAX, BOX_YMAX));
}

// Deconstructor.
CollisionWorld::~CollisionWorld() {
  delete qt;
}

// Update the lines.
void CollisionWorld::updateLines()
{
   detectIntersection();
   updatePosition();
   updateQuadTree();
   lineWallCollision();
}

// Test all line-line pairs to see if they will intersect before the next time
// step.
void CollisionWorld::detectIntersection()
{ 
  // numIntersectionTests = new cilk::reducer_opadd<int>;
  intersections = new cilk::reducer_list_append<IntersectionInfo*>;
  detectIntersectionsOnQuadTree(qt);
  const list<IntersectionInfo*> &intersection_list = intersections->get_value();
  numLineLineCollisions += intersection_list.size();
  list<IntersectionInfo*>::const_iterator current_intersection;

  for(current_intersection = intersection_list.begin(); current_intersection != intersection_list.end(); ++current_intersection) {
    IntersectionInfo* intersection = *current_intersection;
    collisionSolver(intersection->l1, intersection->l2, intersection->intersectionType);
    delete intersection;
  }
  delete intersections;
  // printf("%d\n", numIntersectionTests->get_value());
  
  // delete numIntersectionTests;
}

void CollisionWorld::detectIntersectionsOnQuadTree(QuadTree *qt) {
  // Recurse to all children
  // Loop unrolled for performance
  if(qt->children[0])
    cilk_spawn detectIntersectionsOnQuadTree(qt->children[0]);
  if(qt->children[1])
    cilk_spawn detectIntersectionsOnQuadTree(qt->children[1]);
  if(qt->children[2])
    cilk_spawn detectIntersectionsOnQuadTree(qt->children[2]);
  if(qt->children[3])
    cilk_spawn detectIntersectionsOnQuadTree(qt->children[3]);
  // Run O(n^2) on lines under this QT
  if(!qt->lines.empty())
    bruteForceIntersections(qt, 0, qt->lines.size() - 1);
  cilk_sync;
}

void CollisionWorld::bruteForceIntersections(QuadTree* qt, int lo, int hi) {
  if(lo == hi) {
    Line *l1 = qt->lines[lo];
    int i;
    vector<Line*>::iterator it2;
    for (i = lo + 1; i < qt->lines.size(); ++i) {
      Line *l2 = qt->lines[i];
      IntersectionType intersectionType = efficient_intersect(l1, l2, timeStep);
      // *numIntersectionTests += 1;
      if (intersectionType != NO_INTERSECTION) {
        intersections->push_back(new IntersectionInfo(l1, l2, intersectionType));
        //collisionSolver(l1, l2, intersectionType);
      }
    }
    // Check segments in ancestor quadtree nodes
    QuadTree *ancestor = qt->parent;
    while(ancestor != NULL) {
      for (it2 = ancestor->lines.begin(); it2 != ancestor->lines.end(); ++it2) {
        Line *l2 = *it2;
        IntersectionType intersectionType = efficient_intersect(l1, l2, timeStep);
        // *numIntersectionTests += 1;
        if (intersectionType != NO_INTERSECTION) {
          intersections->push_back(new IntersectionInfo(l1, l2, intersectionType));
          //collisionSolver(l1, l2, intersectionType);
        }
      }
      ancestor = ancestor->parent;
    }
    
  } else {
    int mid = (lo + hi) / 2;
    cilk_spawn bruteForceIntersections(qt, lo, mid);
    bruteForceIntersections(qt, mid+1, hi);
    cilk_sync;
  }
}

// Update line positions.
void CollisionWorld::updatePosition()
{
   double t = timeStep;
   vector<Line*>::iterator it;
   for (it = lines.begin(); it != lines.end(); ++it) {
      Line *line = *it;

      line->p1 += (line->vel * t);
      line->p2 += (line->vel * t);
   }
}

// Dynamically updates quadtree
void CollisionWorld::updateQuadTree() {
  bad_lines = new cilk::reducer_list_append<Line*>;
  for(int i = 0; i < 4; i++)
    updateNode(qt->children[i]);
  list<Line*> segments = bad_lines->get_value();
  // printf("Size: %d\n", segments.size());
  qt->insert(segments);
  qt->push_down();
  delete bad_lines;
}

// Updates quadtree node to make sure all segments are inside the bounding box
void CollisionWorld::updateNode(QuadTree *node) {
  // printf("Updating %d %d\n", node->size, node->lines.size());
  if(node->size > node->lines.size()) {
    for(int i = 0; i < 4; i++) {
      if(node->children[i]->size > 0)
        updateNode(node->children[i]);
    }
  }
  for(vector<Line*>::iterator it = node->lines.begin(); it != node->lines.end(); ) {
    Line *l = *it;
    // printf("%lf %lf %lf %lf ", node->tl.x, node->tl.y, node->br.x, node->br.y);
    // printf("%lf %lf %lf %lf\n", l->p1.x, l->p1.y, l->p2.x, l->p2.y);
    if(l->p1.x < node->tl.x || l->p1.x >= node->br.x || l->p2.x < node->tl.x || l->p2.x >= node->br.x ||
       l->p1.y < node->tl.y || l->p1.y >= node->br.y || l->p2.y < node->tl.y || l->p2.y >= node->br.y) {
      bad_lines->push_back(l);
      it = node->lines.erase(it);
      node->size--;
      QuadTree *ancestor = node->parent;
      while(ancestor) {
        ancestor->size--;
        ancestor = ancestor->parent;
      }
    } else
      it++;
  }
}

void CollisionWorld::preprocessLines() {
   qt->clear();
   qt->updateQuadTree(lines);
}

void CollisionWorld::collisionSolver(Line *l1, Line *l2, IntersectionType
                                     intersectionType)
{
   // Despite our efforts to determine whether lines will intersect ahead of
   // time (and to modify their velocities appropriately), our simplified model
   // can sometimes cause lines to intersect.  In such a case, we compute
   // velocities so that the two lines can get unstuck in the fastest possible
   // way, while still conserving momentum and kinetic energy.
   if (intersectionType == ALREADY_INTERSECTED) {
      Vec p = getIntersectionPoint(l1->p1, l1->p2, l2->p1, l2->p2);
      // Precompute common expressions
      double len1 = (l1->p1 - p).length();
      double len2 = (l1->p2 - p).length();
      if (len1 < len2) {
         l1->vel = ((l1->p2 - p) / len2) * l1->vel.length();
      } else {
         l1->vel = ((l1->p1 - p) / len1) * l1->vel.length();
      }
      len1 = (l2->p1 - p).length();
      len2 = (l2->p2 - p).length();
      if (len1 < len2) {
         l2->vel = ((l2->p2 - p) / len2) * l2->vel.length();
      } else {
         l2->vel = ((l2->p1 - p) / len1) * l2->vel.length();
      }
      return;
   }

   // Compute the collision face/normal vectors
   Vec face;
   Vec normal;
   
   // Compute the mass of each line (we simply use its length).
   double m1 = (l1->p1 - l1->p2).length();
   double m2 = (l2->p1 - l2->p2).length();
   
   if (intersectionType == L1_WITH_L2) {
      Vec v(*l2);
      face = v / m2;
   } else {
      Vec v(*l1);
      face = v / m1;
   }
   normal = face.orthogonal();

   // Obtain each line's velocity components with respect to the collision
   // face/normal vectors.
   double v1Face = l1->vel.dotProduct(face);
   double v2Face = l2->vel.dotProduct(face);
   double v1Normal = l1->vel.dotProduct(normal);
   double v2Normal = l2->vel.dotProduct(normal);

   // Perform the collision calculation (computes the new velocities along the
   // direction normal to the collision face such that momentum and kinetic
   // energy are conserved).
   double newV1Normal = ((m1 - m2) / (m1 + m2)) * v1Normal +
                        (2 * m2 / (m1 + m2)) * v2Normal;
   double newV2Normal = (2 * m1 / (m1 + m2)) * v1Normal +
                        ((m2 - m1) / (m2 + m1)) * v2Normal;

   // Combine the resulting velocities.
   l1->vel = normal * newV1Normal + face * v1Face;
   l2->vel = normal * newV2Normal + face * v2Face;

   return;
}


// Handle line to wall collisions
void CollisionWorld::lineWallCollision()
{
   vector<Line*>::iterator it;
   for (it = lines.begin(); it != lines.end(); ++it) {
      Line *line   = *it;
      bool collide = false;

      // Right side
      if ((line->p1.x > BOX_XMAX  ||
            line->p2.x > BOX_XMAX) &&
            (line->vel.x > 0)) {
         line->vel.x = -line->vel.x;
         collide  = true;
      }
      // Left side
      if ((line->p1.x < BOX_XMIN  ||
            line->p2.x < BOX_XMIN) &&
            (line->vel.x < 0)) {
         line->vel.x = -line->vel.x;
         collide  = true;
      }
      // Top side
      if ((line->p1.y >  BOX_YMAX  ||
            line->p2.y >  BOX_YMAX) &&
            (line->vel.y > 0)) {
         line->vel.y = -line->vel.y;
         collide  = true;
      }
      // Bottom side
      if ((line->p1.y < BOX_YMIN  ||
            line->p2.y < BOX_YMIN) &&
            (line->vel.y < 0)) {
         line->vel.y = -line->vel.y;
         collide  = true;
      }
      // Update total number of collisions
      if (collide == true) {
         numLineWallCollisions++;
      }
   }
}


// Return the total number of lines in the box
unsigned int CollisionWorld::getNumOfLines()
{
   return lines.size();
}


// Add a line into the box
void CollisionWorld::addLine(Line *line)
{
   lines.push_back(line);
}

// Get the i-th line from the box
Line *CollisionWorld::getLine(unsigned int index)
{
   if (index >= lines.size())
      return NULL;
   return lines[index];
}


// Delete all lines in the box
void CollisionWorld::deleteLines()
{
   for (vector<Line*>::iterator it = lines.begin(); it < lines.end(); it++) {
     delete *it;
   }
   lines.clear();
}


// Get total number of line wall collisions
unsigned int CollisionWorld::getNumLineWallCollisions()
{
   return numLineWallCollisions;
}


// Get total number of Line Line collisions;
unsigned int CollisionWorld::getNumLineLineCollisions()
{
   return numLineLineCollisions;
}


